/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file template.cxx
	\brief class implemenation for template

                    \author Jack Elston
             $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>

#include "coopTrack.h"
#include "color.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---Global Variables---->*/
extern bool verbose;
/*<-End Global Variables-->*/

CoopTrack::CoopTrack(String name) : ThreadedPipe(name)
{
	last_tracking_loop.stamp();
	last_custody_req.stamp();

	cout << GREEN << "---------->>>>>       " YELLOW "SEARCH" GREEN "       <<<<<----------"  << ATTROFF << endl;
	fc_mode = SEARCH;
}

void CoopTrack::update() 
{
	int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;

	FD_ZERO(&readFDs);	                    		// zero read fd set
	FD_ZERO(&writeFDs);	                    		// zero write fd set
	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo

	fifo_FDs[INBOARD_RX] > fifo_FDs[OUTBOARD_RX] ? maxFD = fifo_FDs[INBOARD_RX] : maxFD = fifo_FDs[OUTBOARD_RX];
	if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
	if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("CoopTrack::update() select");
				execRate = -1;
			} else
				cout << getName() << "::update() - select interrupted" << endl;
		}
	}
	else {
		if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
		{
			int n = 0;
			n=readFifo(OUTBOARD_RX);
			if(n>0) {
				writeFifo(INBOARD_TX);
			}
		}

		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
			int n = 0;
			n=readFifo(INBOARD_RX);
			if(n>0) {
				uint16_t length = CMD_SIZE(fifo_msg.data);
				if( length != fifo_msg.size) {
					cout << getName() << "::update() - error, length of command != length of buffer" << endl
						<< "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

					if( length < 1000 && length <= fifo_msg.size )
						if( C_calcCheckSum((uint8_t *)buf) == 0 ) 
							cout << "\tCMD: is valid" << endl;
						else
							cout << "\tCMD: is NOT valid" << endl;

					return;
				}

				Command cmd((uint8_t *)fifo_msg.data);

				if( !cmd.isValid() )
					cout << getName() << "::update() - error, invalid command" << endl << cmd ;
				else {
					Client * a_client;
					a_client = shmPtr->clients.getClient(fifo_msg.src);
					if(a_client != NULL) {

						char * dataPtr = CMD_DATAPTR(fifo_msg.data);

						switch(cmd.cmd() ) {
							case CMD_TRACK: {
									 int num_vehicles = cmd.size()/sizeof(VehicleInfo_t);

									 me->telemetry.navigation.OBmode = OB_OFF;
									 me->telemetry.navigation.OBtype = OB_COOP_TRACK;

									 fc_mode = SEARCH;
									 tracked_vehicles.clear();
									 children.clear();

									 if( num_vehicles > 0 ) {
										 int sum_sub=0;
										 for(int j=0;j<num_vehicles;j++){
											 if((a_client = shmPtr->clients.getClient(((VehicleInfo_t*)dataPtr)[j].id)) != NULL) {
												 if(tracked_vehicles.add(a_client)) {
													 cout << "requresting TELEM stream" << endl;
													 subscribe(CMD_TELEM, a_client, true, INBOARD_TX);
													 sum_sub++;
												 }
											 }
										 }
										 if( sum_sub > 0 ) {
											 me->telemetry.navigation.OBmode = OB_INIT;
											 ACK(INBOARD_TX);
											 cout << "---------->>>>>  COOPERATIVE TRACK      <<<<<----------" << endl;
											 fc_mode = TRACK;

											 cout << "TRACKING " << tracked_vehicles.getNumClients() << " VEHICLES" << endl;
										 } else {
											 me->telemetry.navigation.OBmode = OB_ERROR;
											 NACK(INBOARD_TX);
											 tracked_vehicles.clear();
											 children.clear();
										 }
									 } else
										 ACK(INBOARD_TX);
								 } break;
							case CMD_CUSTODY: {
									Custody_t * custody  = (Custody_t*)dataPtr;
									Client * me = shmPtr->clients.getClient(0);
									cout << "CUSTODY RESPONSE from " << a_client->name << " with " << inet_ntoa(custody->parent) <<  endl;
									if(custody->parent.s_addr == shmPtr->clients.getAddr(me).s_addr) {
										cout << "I have custody" << endl;
										a_client->state = (uint8_t)MY_CUSTODY;
										children.add(a_client);
										if(children.getNumClients() > 1) {
											cout << "---------->>>>>  CHILD_COORDINATE  <<<<<----------" << endl;
											fc_mode = CHILD_COORDINATE;
											for(int i=0; i< children.getNumClients(); i ++)
												children.getClient(i)->state = (uint8_t)COORDINATE_REQ;
											coordinateChildren(true);
										}
									}
									else {
										a_client->state = (uint8_t)OTHER_CUSTODY;
										if(children.getClient(a_client->name) != NULL) {
											children.remove(a_client);
											cout << "removed child: " << a_client->name << endl;
											cout << "---------->>>>>       TRACK        <<<<<----------" << endl;
											fc_mode = TRACK;
										}
									}
								} break;
							case CMD_ACK: {
									cout << "CMD_ACK from " << a_client->name << endl;
									switch(fc_mode) {
										case CHILD_COORDINATE:
											children.getClient(a_client->name)->state = (uint8_t)COORDINATED;
											if(children.getClient(0)->state == (uint8_t)COORDINATED &&
													children.getClient(1)->state == (uint8_t)COORDINATED) {
												cout << "---------->>>>>       TRACK        <<<<<----------" << endl;
												fc_mode = TRACK;
											}
											break;
										case CHILD_UNCOORDINATE:
											if(children.getClient(a_client->name) != NULL) {
												children.getClient(a_client->name)->state = (uint8_t)UNCOORDINATED;
												if(children.getClient(0)->state == (uint8_t)UNCOORDINATED &&
														children.getClient(1)->state == (uint8_t)UNCOORDINATED) {
													cout << "---------->>>>>      REM_CHILD     <<<<<----------" << endl;
													fc_mode = REM_CHILD;
												}
											}
											break;
										default:
											if(a_client->state == (uint8_t)REQUESTING_TRACK) {
												a_client->state = (uint8_t)TRACKING;
												cout << "---------->>>>>     REM_TARGET     <<<<<----------" << endl;
												fc_mode = REM_TARGET;
											}
											break;
									}
								} break;
							case CMD_FAIL: {
									cout << "CMD_FAIL from " << a_client->name << endl;
									switch(fc_mode) {
										default:
											if(a_client->state == (uint8_t)REQUESTING_TRACK) {
												a_client->state = (uint8_t)UNAVAILABLE;
											}
											break;
									}
								} break;
/*
							case CMD_GPS_TELEM: {
									GPSStruct_t * telemetry  = (GPSStruct_t*)dataPtr;
									Client * tracked_client = tracked_vehicles.getClient(a_client->name);
									if(tracked_client != NULL) {
										tracked_client->telemetry.pos.lat=telemetry->pos.lat;
										tracked_client->telemetry.pos.lon=telemetry->pos.lon;
										tracked_client->telemetry.track.tru=telemetry->track.tru;
										tracked_client->telemetry.airdata.trueAirSpeed=telemetry->sog.value;
									}
								} break;
*/
							default:
								writeFifo(OUTBOARD_TX);
								break;
						}
					}
				}
			}
		}
	}
	currentStamp.stamp();
	if((currentStamp - last_tracking_loop ) > (TRACKING_LOOP_PERIOD * SEC2MICRO) ) {
		bool got_info = true;
		for(int i=0; i< tracked_vehicles.getNumClients(); i ++)
			if(((tracked_vehicles.getClient(i))->telemetry.pos.lat) == 0.0) {
				cout << "requresting TELEM stream" << endl;
				subscribe(CMD_TELEM, tracked_vehicles.getClient(i), true, INBOARD_TX);
				got_info = false;
			}
		if((tracked_vehicles.getNumClients() > 0) && got_info) { trackTargets(); } 

		last_tracking_loop.stamp();
	}
	if(fc_mode == REQ_CHILD || fc_mode == REM_CHILD) {
		if((currentStamp - last_custody_req ) > (CUSTODY_REQ_PERIOD * SEC2MICRO) ) {
			manageCustody();
			last_custody_req.stamp();
		}
	}
}

void CoopTrack::findMAVs()
{
	cout << "findMAVs()" << endl;
	Client * a_client;
	int i = 1;
	while( (a_client = shmPtr->clients.getClient(i)) != NULL) {
		if(a_client->capabilities.find(CMD_CUSTODY)) {
			cout << "CLIENT " << a_client->name << " POSSIBLE" << endl;
			a_client->state = (uint8_t)CUSTODY_REQ;
		}
		else 
			cout << "CLIENT " << a_client->name << " NOT POSSIBLE" << endl;
		i++;
	}
}

void CoopTrack::manageCustody()
{
	cout << "manageCustody()" << endl;
	Client * a_client;
	int i = 1;
	while( (a_client = shmPtr->clients.getClient(i)) != NULL) {
		if(a_client->state == (uint8_t)CUSTODY_REQ) {
			cout << "request custody of " << a_client-> name << endl;
			reqCustody(a_client, true);
		}
		else if(a_client->state == (uint8_t)CUSTODY_DROP) {
			cout << "drop custody of " << a_client-> name << endl;
			reqCustody(a_client, false);
		}
		else if(children.getNumClients() < 2) {
			if(a_client->state == 0) {
				if(a_client->capabilities.find(CMD_CUSTODY)) {
					cout << "CLIENT " << a_client->name << " POSSIBLE" << endl;
					a_client->state = (uint8_t)CUSTODY_REQ;
				}
			}
		}
		else
			cout << a_client->name << " state: " << (int)a_client->state << endl;
		i++;
	}
}

void CoopTrack::recruitSAVs()
{
	cout << "recruitSAVs()" << endl;
	cout << "TRYING TO FIND SAV" << endl;
	Client * a_client;
	int i = 1;
	while( (a_client = shmPtr->clients.getClient(i)) != NULL) {
		if(a_client->capabilities.find(CMD_TRACK)) {
			if(a_client->state != (uint8_t)UNAVAILABLE) {
				cout << "CLIENT " << a_client->name << " POSSIBLE" << endl;
				a_client->state = (uint8_t)REQUESTING_TRACK;
				transferTrack(a_client);
			}
		}
		i++;
	}
}

void CoopTrack::trackTargets() {
	cout << "trackTargets()" << endl;
	GPSLatLon_t orbit_point;
	orbit_point.lat=0.0;
	orbit_point.lon=0.0;

	for(int j=0;j<tracked_vehicles.getNumClients();j++) {
		orbit_point.lat += tracked_vehicles.getClient(j)->telemetry.pos.lat;
		orbit_point.lon += tracked_vehicles.getClient(j)->telemetry.pos.lon;
		cout << "tracking " << tracked_vehicles.getClient(j)->name << " (" << tracked_vehicles.getClient(j)->telemetry.pos.lat << "," << tracked_vehicles.getClient(j)->telemetry.pos.lon << ")" << endl;
	}

	orbit_point.lat /= tracked_vehicles.getNumClients();
	orbit_point.lon /= tracked_vehicles.getNumClients();

	if(children.getNumClients() > 0)
		childrenTrack(orbit_point);
	else {
		cout << "---------->>>>>     REQ_CHILD      <<<<<----------" << endl;
		fc_mode = REQ_CHILD;
		findMAVs();
	}

	Waypt_t * waypoint = (Waypt_t*)CMD_DATAPTR(fifo_msg.data);

	me->telemetry.navigation.OBmode = OB_ORBIT; 

	waypoint[0].number   = ORBIT_WAYPOINT;
	waypoint[0].pos.lat  = orbit_point.lat;
	waypoint[0].pos.lon  = orbit_point.lon;
	waypoint[0].alt      = TRACKING_ALT;
	waypoint[0].orbitRad = -ORBIT_RADIUS;
	waypoint[0].next     = ORBIT_WAYPOINT + 1;

	waypoint[1].number   = ORBIT_WAYPOINT + 1;
	waypoint[1].pos.lat  = me->telemetry.gps.pos.lat;
	waypoint[1].pos.lon  = me->telemetry.gps.pos.lon;
	waypoint[1].alt      = TRACKING_ALT;
	waypoint[1].orbitRad = 0;
	waypoint[1].next     = ORBIT_WAYPOINT;

	CMD_CMD(fifo_msg.data) = CMD_FP;
	CMD_NUM(fifo_msg.data) = 2*sizeof(Waypt_t);
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	fifo_msg.size = CMD_SIZE(fifo_msg.data);
	writeFifo(OUTBOARD_TX);
}

void CoopTrack::reqCustody(Client * a_client, bool toggle) {
	cout << "reqCustody()" << endl;
	Client * me = shmPtr->clients.getClient(0);
	CMD_CMD(fifo_msg.data) = CMD_CUSTODY;

	CMD_NUM(fifo_msg.data) = sizeof(Custody_t);
	Custody_t * request = (Custody_t*)(fifo_msg.data + CMD_HEADER_SIZE);

	(request->parent).s_addr = (shmPtr->clients.getAddr(me,a_client)).s_addr;
	request->requesting=toggle;
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
	fifo_msg.size = CMD_SIZE(fifo_msg.data);
	writeFifo(INBOARD_TX);
	cout << "SENT REQUEST TO " << a_client->name << endl;
}

void CoopTrack::coordinateChildren(bool toggle) {
	cout << "coordinateChildren()" << endl;

	if(toggle)
		cout << "Coordinating children" << endl;
	else
		cout << "Uncoordinating children" << endl;

	CMD_CMD(fifo_msg.data) = CMD_COORDINATE;
	CMD_NUM(fifo_msg.data) = sizeof(Coordinate_t);
	Coordinate_t * request = (Coordinate_t*)CMD_DATAPTR(fifo_msg.data);

	request->partner.s_addr = shmPtr->clients.getAddr(children.getClient(1)).s_addr;
	request->requesting = toggle;
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	fifo_msg.dest.s_addr = shmPtr->clients.getAddr(children.getClient(0)).s_addr;
	fifo_msg.size = CMD_SIZE(fifo_msg.data);
	writeFifo(INBOARD_TX);

	request->partner.s_addr = shmPtr->clients.getAddr(children.getClient(0)).s_addr;
	request->requesting = toggle;
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	fifo_msg.dest.s_addr = shmPtr->clients.getAddr(children.getClient(1)).s_addr;
	writeFifo(INBOARD_TX);
}

void CoopTrack::childrenTrack(GPSLatLon_t orbit_point) {
	cout << "childrenTrack()" << endl;

	if(children.getNumClients() > 1) {
		if(fc_mode == CHILD_COORDINATE) {
			coordinateChildren(true);
			return;
		}
	}

	bool convoy_split = false;

	CMD_CMD(fifo_msg.data) = CMD_TELEM;
	CMD_NUM(fifo_msg.data) = sizeof(Telemetry_t);
	Telemetry_t * telemetry = (Telemetry_t*)CMD_DATAPTR(fifo_msg.data);

	Client * vehicle1 = tracked_vehicles.getClient(0);
	Client * vehicle2 = NULL;
	Client * target= NULL;
	Client * other_target= NULL;

	if(tracked_vehicles.getNumClients() == 2) {
		vehicle2 = tracked_vehicles.getClient(1);

		float r = vehicle1->rangeTo(vehicle2);

		if(r > 200) {
			cout << "---> SPLIT CONVOY <---" << endl;
			convoy_split=true;
		}
	}

	if(convoy_split) {
		if( vehicle1->(children.getClient(0)) > vehicle2->rangeTo(children.getClient(0)) ) {
			target = vehicle2;
			other_target = vehicle1;
		}
		else {
			target = vehicle1;
			other_target = vehicle2;
		}
		telemetry->pos.lat = target->telemetry.pos.lat;
		telemetry->pos.lon = target->telemetry.pos.lon;
		telemetry->track.tru = other_target->telemetry.track.tru;
		telemetry->airdata.trueAirSpeed = other_target->telemetry.airdata.trueAirSpeed;
	}
	else {
		//if(fc_mode == REQ_SAV) fc_mode = TRACK; FIXME - complicated case
		telemetry->pos.lat = orbit_point.lat;
		telemetry->pos.lon = orbit_point.lon;
		telemetry->track.tru = vehicle1->telemetry.track.tru;
		telemetry->airdata.trueAirSpeed = vehicle1->telemetry.airdata.trueAirSpeed;
	}

	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	fifo_msg.size = CMD_SIZE(fifo_msg.data);

	fifo_msg.dest.s_addr = shmPtr->clients.getAddr(children.getClient(0)).s_addr;
	writeFifo(INBOARD_TX);

	if(children.getNumClients() > 1) {
		if(convoy_split) {
			telemetry->pos.lat = other_target->telemetry.pos.lat;
			telemetry->pos.lon = other_target->telemetry.pos.lon;
			telemetry->track.tru = other_target->telemetry.track.tru;
			telemetry->airdata.trueAirSpeed = other_target->telemetry.airdata.trueAirSpeed;
			CMD_CSUM(fifo_msg.data) = 0;
			CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);
		}
		fifo_msg.dest.s_addr = shmPtr->clients.getAddr(children.getClient(1)).s_addr;
		writeFifo(INBOARD_TX);
	}

	if(convoy_split)
		if(fc_mode < REM_TARGET)
			recruitSAVs();
	if(fc_mode > CHILD_COORDINATE)
		finishTransfer();
}

void CoopTrack::transferTrack(Client * a_client) 
{
	cout << "transferTrack()" << endl;

	cout << "transfering tracking to: " << a_client->name << endl;
	Client * removed_vehicle = tracked_vehicles.getClient(1);
	removed_vehicle->state = (uint8_t)STREAM_REM;
	VehicleInfo_t * vehicle = (VehicleInfo_t*)CMD_DATAPTR(fifo_msg.data);
	strcpy(vehicle->id,removed_vehicle->name.c_str());

	fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr; 
	CMD_CMD(fifo_msg.data) = CMD_TRACK;
	CMD_NUM(fifo_msg.data) = sizeof(VehicleInfo_t);
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	fifo_msg.size = CMD_SIZE(fifo_msg.data);
	writeFifo(INBOARD_TX);
}

void CoopTrack::finishTransfer() 
{
	cout << "finishTransfer()" << endl;

	switch(fc_mode) {
		case(REM_TARGET):
			cout << "transfering vehicle: " << tracked_vehicles.getClient(1)->name << endl;
			subscribe(CMD_TELEM, tracked_vehicles.getClient(1), false, INBOARD_TX);
			tracked_vehicles.remove(tracked_vehicles.getClient(1));
			if(children.getNumClients() > 1) {
				cout << "---------->>>>> CHILD_UNCOORDINATE <<<<<----------" << endl;
				for(int i=0; i< children.getNumClients(); i ++)
					children.getClient(i)->state = (uint8_t)UNCOORDINATE_REQ;
				fc_mode = CHILD_UNCOORDINATE;
			}
			else {
				cout << "---------->>>>>       TRACK        <<<<<----------" << endl;
				fc_mode = TRACK;
			}
			break;
		case(CHILD_UNCOORDINATE):
			cout << "uncoordinating children" << endl;
			coordinateChildren(false);
			break;
		case(REM_CHILD):
			cout << "removing child " << children.getClient(1)->name << endl;
			shmPtr->clients.getClient(children.getClient(1)->name)->state = (uint8_t)CUSTODY_DROP;
			break;
		default:
			break;
	}
}
